/**
 * Simple Read
 * 
 * Read data from the serial port and change the color of a rectangle
 * when a switch connected to a Wiring or Arduino board is pressed and released.
 * This example works with the Wiring / Arduino program that follows below.
 */


import processing.serial.*;
PrintWriter output;
Serial myPort;  // Create object from Serial class

         // horizontal position of the graph
 
 //VARIABLES DISPLAYED
float jerkX = 0;      // Data received from the serial port
float jerkY = 0;
float jerkZ = 0;
float rotX = 0;
float rotY = 0;
float rotZ = 0;
float emgAmp1 = 0;
float emgAmp2 = 0;
float emg1RMS = 0;
float emg2RMS = 0;
float emg1PeakFreq = 0;
float emg2PeakFreq = 0;
float[] emg1FFT = new float[80];
float[] emg2FFT = new float[80];
float mode = 0;


//for calibration mode
float jerkSens = 0;
float rotSens = 0;
float ampSens = 0;
float rmsSens = 0;
float modSens = 0;
float jerkTotAvg = 0;
float rotTotAvg = 0;
float ampTotAvg = 0;
float rmsTotAvg = 0;
float modTotAvg = 0;

//for recording mode
float gestureNum = 0;
float sampleNum = 0;
String value = "";
float prevSampleNum = 0;
//String value = "";
boolean firstReading;
float[] Patterns = {0, 0, 0, 0, 0, 0};// Data received from the serial port. Contains the patterns' scores

String inString; //temp for parsing
int xPos = width/10;  //initial location EMG real time feed

int numWatches = 181; //watch being defined as a variable being recorded and sent to here from teensy
String[] input = new String[numWatches];
void setup() 
{
  xPos = width/10 + 410;
  size(1000, 800);
  textSize(20);
  println(Serial.list());
  String portName = "COM7";
  myPort = new Serial(this, portName, 115200);
  myPort.bufferUntil('\n');
 background(220,220,220);
 output = createWriter( "data.txt" );
 firstReading = true; //ignore the first reading so that you dont lose information
}

void draw(){
  if( (mode < 0) || (mode > 3) ){
   background(195,50,50);
    fill(0,0,0);
     textSize(30);text("No connection established", width/2-220, height/2);  textSize(20);
 }
  
  if(mode ==0){
    background(200,200,255); 
    fill(0,0,0);
    textSize(30);text("sleeping =.=", width/2 - 100, height/2);  textSize(20);
     
  }
  if(mode ==1){
      
    //dynamic background
   
    fill(220,220,220);
    rect(0,0, width-2,height/2);
    //line(width/3,height*3/4, width*2/3, height*3/4);
  
  
      fill(0,0,0);
    //IMU Prints
    rect(30 , height/3 , 30, -8*jerkX); 
    rect(80, height/3, 30, -8*jerkY);
    rect(130, height/3, 30, -8*jerkZ);
    rect(180, height/3 , 30, -10*rotX);
    rect(230, height/3 , 30, -10*rotY);
    rect(280, height/3 , 30, -10*rotZ);
    textSize(30);text("IMU", 130, 30);  textSize(20);
    text("jerkX", 30, 50);
    text("jerkY", 80, 50);
    text("jerkZ", 130, 50);
    text("rotX", 180, 50);
    text("rotY", 230, 50);
    text("rotZ", 280, 50);
    
    //Pattern Prints
    textSize(30);text("Patterns", 600, 27);  textSize(20);
    rect(370 , height/3 , 30, -Patterns[0]); 
    rect(475, height/3, 30, -Patterns[1]);
    rect(560, height/3, 30, -Patterns[2]);
    rect(670 , height/3 , 30, -Patterns[3]); 
    rect(770, height/3, 30, -Patterns[4]);
    rect(870, height/3, 30, -Patterns[5]);
    text("Forward", 360, 50);
    text("Backward", 450, 50);
    text("Left", 560, 50);
    text("Right", 650, 50);
    text("Stop", 740, 50);
    text("Slow Down", 830, 50);
    text(int(Patterns[0]), 360, height/3 + 20);
    text(int(Patterns[1]), 470, height/3 + 20);
    text(int(Patterns[2]), 570, height/3 + 20);
    text(int(Patterns[3]), 670, height/3 + 20);
    text(int(Patterns[4]), 770, height/3 + 20);
    text(int(Patterns[5]), 870, height/3 + 20);
    
    //EMG PRINTS
    fill(220,220,220);  rect(width/2,height/2, width,height); fill(0,0,0);//dynamic background
     
      //PEAK Value
  
     textSize(15);text("Mod Freq:", width/2 + 80 , height/2+190);
        fill(255,0,0);
     textSize(15);text("Mod Freq:", width/2 + 320 , height/2+190);
     textSize(15);text((int)emg1PeakFreq, width/2 + 410 , height/2+190);textSize(20);fill(0,0,0); 
     textSize(15);text((int)emg2PeakFreq, width/2 + 160 , height/2+190);textSize(20);
    
    textSize(30);text("EMG Amplitudes", width/10 , height/2+50);  
    textSize(30);text("EMG FFT", width/10*7 , height/2+50);  
    fill(255,0,0);textSize(10);text("Channel 2", width/10 - 100 , height/2+100);fill(0,0,0); 
    textSize(10);text("Channel 1", width/10 - 100 , height/2+80);  textSize(20);
    fill(255,0,0);textSize(10);text("Channel 2", width/10*7 - 150 , height/2+100);fill(0,0,0); 
    textSize(10);text("Channel 1", width/10*7 - 150 , height/2+80);  textSize(20);
    
    //textSize(15);text("Amplitude", width/10 - 90 , height - 200);  textSize(20);
    textSize(15);text("RMS", width/10 - 45 , height - 65);  textSize(20);
    
    //EMG ONE
      //line(width/10,height*3/4, width/10+330, height*3/4);
     // ellipse(xPos, height*3/4 + 65 - (emgAmp1)*20, 2, 6);
      
    //EMG TWO
      stroke(255,0,0);
      //ellipse(xPos, height*3/4 + 65 - (emgAmp2)*20, 2, 6);
      stroke(0,0,0);
    
    //RMS ONE value{
      //line(width/10,height*3/4+140, width/10+330, height*3/4+140);
      ellipse(xPos, height*3/4 +120 - int(emg1RMS)*4, 2, 3);
      
      //RMS TWO value{
      stroke(255,0,0);
      ellipse(xPos, height*3/4 +120 - int(emg2RMS)*4, 2, 3);
      stroke(0,0,0);
      
    //}
      //FFT data
      for(int i = 0; i < 80; i++){
        //text(input[i], 360, height/3 + 20);
        //ellipse(i*2+200, height*3/4 + 50 - int(input[i])/6, 2, 7);
        line(i*2+580, height*3/4 + 160 - int(emg1FFT[i])/25, i*2+580, height*3/4+160);
        //point(i*2+200, height*3/4+50 - int(input[i])/6);
      }
      stroke(255,0,0);
      for(int i = 0; i < 80; i++){
        //text(input[i], 360, height/3 + 20);
        //ellipse(i*2+200, height*3/4 + 50 - int(input[i])/6, 2, 7);
        line(i*2+800, height*3/4 + 160 - int(emg2FFT[i])/25, i*2+800, height*3/4+160);
        //point(i*2+200, height*3/4+50 - int(input[i])/6);
      }
      stroke(0,0,0);
    // at the edge of the emg graph, go back to the beginning:
    if (xPos >= width/10+330) {
      xPos = width/10;
      background(220,220,220);
    }//else iterate xPos
    xPos = xPos + 1;
    determinePattern();
  }
  if(mode == 2){
    fill(255,200,200);
    rect(0,0, width/2,height);
    rect(width/2,0, width,height*3/4);
    //background(255,200,200); 
    fill(0,0,0);
    textSize(30);text("Calibrating", width - 300, height/8-70);  textSize(20);
    text("jerk sens", 740, 100);
    text("rot sens", 750, 130);
    text("amp sens",750, 160);
    text("rms sens", 750, 190);
    text("mod sens", 750, 220);
    text(jerkSens, 840, 100);
    text(rotSens, 840, 130);
    text(ampSens, 840, 160);
    text(rmsSens, 840, 190);
    text(modSens, 840, 220);      
    
    text("jerk avg", 510, 100);
    text("rot avg", 520, 130);
    text("amp avg",510, 160);
    text("rms avg", 515, 190);
    text("mod avg", 510, 220);
    text(jerkTotAvg, 600, 100);
    text(rotTotAvg, 600, 130);
    text(ampTotAvg, 600, 160);
    text(rmsTotAvg, 600, 190);
    text(modTotAvg, 600, 220);  
    
    fill(0,0,0);
    //IMU Prints
    rect(30 , height/3 , 30, -10*jerkX); 
    rect(80, height/3, 30, -10*jerkY);
    rect(130, height/3, 30, -10*jerkZ);
    rect(180, height/3 , 30, -15*rotX);
    rect(230, height/3 , 30, -15*rotY);
    rect(280, height/3 , 30, -15*rotZ);
    textSize(30);text("IMU", 130, 30);  textSize(20);
    text("jerkX", 30, 50);
    text("jerkY", 80, 50);
    text("jerkZ", 130, 50);
    text("rotX", 180, 50);
    text("rotY", 230, 50);
    text("rotZ", 280, 50);
    
          ellipse(xPos, height*3/4 +120 - int(emg1RMS)*4, 2, 3);
       textSize(30);text("RMS", width - 270, height-170);  textSize(20);
      //RMS TWO value{
      stroke(255,0,0);
      ellipse(xPos, height*3/4 +120 - int(emg2RMS)*4, 2, 3);
      stroke(0,0,0);
       
          // at the edge of the emg graph, go back to the beginning:
    if (xPos >= width-50) {
      xPos = width/2+100;
      background(255,200,200);
    }//else iterate xPos
          xPos = xPos + 1;

  }
  if(mode == 3){
    if(sampleNum == 0){ background(200,255,200);} 
    fill(255,255,255);
    rect(width/2+80, 0, width, height);
    fill(0,0,0);
    textSize(30);text("recording", width*3/4, height/2);  textSize(20);
    text("recording for gesture #: ", 590, 500);
    text(int(gestureNum), 840, 500);
    text("sample number: ", 590, 600);
    text(int(sampleNum), 840, 600);
    
    int sp = 45;
    int off = -25;
    textSize(15);
    text("jX",1*sp+off,15);
    text("jY",2*sp+off,15);
    text("jZ",3*sp+off,15);
    text("rX",4*sp+off,15);
    text("rY",5*sp+off,15);
    text("rZ",6*sp+off,15);
    text("e1",7*sp+off,15);
    text("e2",8*sp+off,15);
    text("re1",9*sp+off,15);
    text("re2",10*sp+off,15);
    text("m1",11*sp+off,15);
    text("m2",12*sp+off,15);
    
    textSize(30);
    
    if(prevSampleNum!= sampleNum){
      //value = "NEW LINE\n";
      //output.println( value );
      
      textSize(15);
      text(int(jerkX), 1*sp+off,sampleNum*18+30);
      text(int(jerkY), 2*sp+off,sampleNum*18+30);
      text(int(jerkZ), 3*sp+off,sampleNum*18+30);
      text(int(rotX), 4*sp+off,sampleNum*18+30);
      text(int(rotY), 5*sp+off, sampleNum*18+30);
      text(int(rotZ), 6*sp+off, sampleNum*18+30);
      text(int(emgAmp1), 7*sp+off, sampleNum*18+30);
      text(int(emgAmp2), 8*sp+off, sampleNum*18+30);
      text(int(emg1RMS), 9*sp+off, sampleNum*18+30);
      text(int(emg2RMS), 10*sp+off, sampleNum*18+30);
      text(int(emg1PeakFreq), 11*sp+off, sampleNum*18+30);
      text(int(emg2PeakFreq), 12*sp+off, sampleNum*18+30);

      
      textSize(30);
    }
    
    if(prevSampleNum != sampleNum){
      value = "";
      value += str(jerkX);value += " ";
      value += str(jerkY);value += " ";
      value += str(jerkZ);value += " ";
      value += str(rotX);value += " ";
      value += str(rotY);value += " ";
      value += str(rotZ);value += " ";
      value += str(emgAmp1);value += " ";
      value += str(emgAmp2);value += " ";
      value += str(emg1RMS);value += " ";
      value += str(emg2RMS);value += " ";
      value += str(emg1PeakFreq);value += " ";
      value += str(emg2PeakFreq);value += "\n";
      //if(prevGestureNum != gestureNum){
       //  value += "\n"; 
      //}
      if(value != null){
        output.println( value );
        output.flush();
      }
    }
     prevSampleNum = sampleNum;
  }
     
}

void serialEvent (Serial myPort) {
  if(!firstReading){   //if not the first reading then extract info
    println("recieved info");
    inString = myPort.readStringUntil('\n');
    inString = trim(inString);
    println(inString);
    input = split(inString, "\t");    
    println("Setting values");  
    extractInfo();
}
  if(firstReading){ //else, ignore it and update firstReading variable
    firstReading = false;
    println("recieved FIRST info");
    inString = myPort.readStringUntil('\n');
    inString = trim(inString);
    println(inString);
  }
}
void extractInfo(){
//MODE 0 data
  //1-179: not used
  //180: mode

//MODE 1 data[]
  //   0    1        2      3    4    5      6        7        8        9        10          
  //[jerkX, jerkY, jerkZ, rotX, rotY, rotZ, emgAmp1, emgAmp2, emg1RMS, emg2RMS, emg1PeakFreq, 
  //  11           12            13             14              15            16              17
  //emg2PeakFreq, pattern1Score, pattern2Score, pattern3Score, pattern4Score, pattern5Score, pattern6Score
  //FFT data 1: 20-99
  //FFT data 2: 100-179
  //180: mode
  
//MODE 2 data[]
  //   0    1        2      3    4    5      6        7        8        9        10          
  //[jerkX, jerkY, jerkZ, rotX, rotY, rotZ, emgAmp1, emgAmp2, emg1RMS, emg2RMS, emg1PeakFreq, 
  //  11           12        13        14        15       16           17        18
  //emg2PeakFreq, jerkSens, rotSens, ampSens, rmsSens, modSens,     jerkTotAvg, rotTotAvg,
  //  19        20         21          22
  //ampTotAvg,  rmsTotAvg,  modTotAvg
  
  //23-179: not used
  //180: mode

//MODE 3 data[]
  //   0    1        2      3    4    5      6        7        8        9        10          
  //[jerkX, jerkY, jerkZ, rotX, rotY, rotZ, emgAmp1, emgAmp2, emg1RMS, emg2RMS, emg1PeakFreq, 
  //  11           12            13      
  //emg2PeakFreq,  gestureNum,  sampleNum
  //14-179: not used
  //180: mode

 mode = float(input[180]);
 
 if(mode==3)
  jerkX = float(input[0]);
   jerkY = float(input[1]);
   jerkZ = float(input[2]);
   rotX = float(input[3]);
   rotY = float(input[4]);
   rotZ = float(input[5]);
   emgAmp1 = float(input[6]);
   emgAmp2 = float(input[7]);
   emg1RMS = float(input[8]);
   emg2RMS = float(input[9]);
   emg1PeakFreq = float(input[10]);
   emg2PeakFreq = float(input[11]);
   gestureNum = float(input[12]);
   sampleNum = float(input[13]);
   
 if(mode ==2){
   jerkSens = float(input[12]);
   rotSens = float(input[13]);
   ampSens = float(input[14]);
   rmsSens = float(input[15]);
   modSens = float(input[16]);
   jerkTotAvg = float(input[17]);
   rotTotAvg = float(input[18]);
   ampTotAvg = float(input[19]);
   ampTotAvg = float(input[20]);
   modTotAvg = float(input[21]);
   //this is really the resultant readings, not quite the same
   jerkX = float(input[0]);
   jerkY = float(input[1]);
   jerkZ = float(input[2]);
   rotX = float(input[3]);
   rotY = float(input[4]);
   rotZ = float(input[5]);
   emgAmp1 = float(input[6]);
   emgAmp2 = float(input[7]);
   emg1RMS = float(input[8]);
   emg2RMS = float(input[9]);
   emg1PeakFreq = float(input[10]);
   emg2PeakFreq = float(input[11]);
   
   
 }
 if(mode ==1){
   jerkX = float(input[0]);
   jerkY = float(input[1]);
   jerkZ = float(input[2]);
   rotX = float(input[3]);
   rotY = float(input[4]);
   rotZ = float(input[5]);
   emgAmp1 = float(input[6]);
   emgAmp2 = float(input[7]);
   emg1RMS = float(input[8]);
   emg2RMS = float(input[9]);
   emg1PeakFreq = float(input[10]);
   emg2PeakFreq = float(input[11]);
   Patterns[0] = float(input[12]);
   Patterns[1]= float(input[13]);
   Patterns[2] = float(input[14]);
   Patterns[3] = float(input[15]);
   Patterns[4] = float(input[16]);
   Patterns[5] = float(input[17]);
   //Assign EMG_1 FFT
   for(int i = 20; i < 100; i++){//written to show there are 255 values being recorded. sum is 282
     emg1FFT[i-20] = float(input[i]);
   }
   //Assign EMG_2 FFT
   for(int i = 100; i < 180; i++){//written to show there are 255 values being recorded. sum is 583
     emg2FFT[i-100] = float(input[i]);
   } 
 }
}

void determinePattern(){
//FIND OUT WHICH PATTERN HAD THE LARGEST SCORE
  if(Patterns[0] == max(Patterns) && Patterns[0] != 0){
    textSize(30);text("FORWARD", width*2/4 - 140, height/4+120);  textSize(20);
  }
  else if(Patterns[1] == max(Patterns) && Patterns[1] != 0){
    textSize(30);text("BACKWARD", width*2/4 - 140, height/4+120);  textSize(20);
  }
  else if(Patterns[2] == max(Patterns) && Patterns[2] != 0){
    textSize(30);text("LEFT", width*2/4 - 140, height/4+120);  textSize(20);
  }
  else if(Patterns[3] == max(Patterns)&& Patterns[3] != 0){
    textSize(30);text("RIGHT", width*2/4 - 140, height/4+120);  textSize(20);
  }
  else if(Patterns[4] == max(Patterns)&& Patterns[4] != 0){
    textSize(30);text("STOP", width*2/4 - 140, height/4+120);  textSize(20);
  }
  else if(Patterns[5] == max(Patterns)&& Patterns[5] != 0){
    textSize(30);text("SLOW DOWN", width*2/4 - 140, height/4+120);  textSize(20);
  }
  else{
    textSize(30);text("NO GESTURE DETECTED", width*2/4 - 50, height/4+120);  textSize(20);
  }
}


  


